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ABSTRACT 

In  this  note  we  consider  the  problem  of  planning  a  continuous  collision- 
free  motion  of  a  robot  system  B  having  it  degrees  of  freedom  and  moving 
amidst  a  collection  of  known  obstacles  of  bounded  local  complexity.  By  this 
we  mean  that  the  obstacles  (and  B  itself)  are  bounded  by  algebraic  surfaces  of 
uniformly  bounded  degree  and  that,  for  each  (hence  every)  r,  no  ball  of 
radius  r  can  touch  more  than  a  fixed  number  of  obstacles.  Wc  show  that 
under  this  assumption  (which  is  likely  to  be  satisfied  in  many  cases  arising  in 
practice)  the  motion  planning  problem  for  B  can  easily  be  solved  in  time 
linear  in  the  number  of  spheres  of  fixed  radius  necessary  to  cover  the  whole 
environment  in  which  B  is  free  to  move. 

1.    Introduction 

Let  fl  be  a  robot  system  consisting  of  a  collection  of  rigid  subparts  (some  of  which  may 
be  attached  to  each  other  at  joints  while  others  may  move  independently)  having  a  total  of  k 
degrees  of  freedom,  and  suppose  that  B  is  free  to  move  in  a  two  or  three  dimensional  space 
V  amidst  a  collection  of  obstacles  whose  geometry  is  known  to  the  robot  system.  The  motion 
planning  problem  for  B  can  be  defined  as  follows:  Given  an  initial  position  Z,  and  a  desired 


final  position  Z^ol  B,  determine  whether  there  exists  a  continuous  obstacle-avoiding  motion 
of  B  from  Z,  to  Z2,  and  if  so  plan  such  a  motion. 

This  problem  has  been  studied  in  many  recent  papers  (cf.  [LPW],  [Mo],  [Re],  [SSI], 
[SS2],  [SS3],  [SA],  [SS4],  [Ya],  [OY],  [OSY],  [OSYl],  [OSY2],  [LSI],  [LS2],  [KS],  [SiS], 
[HJWl],  [HJW2],  [HW],  [HSS],  [JP],  [Brl],  [Br2],  [BLP],  [LP]).  It  is  equivalent  to  the 
problem  of  calculating  the  path-connected  components  of  the  (^-dimensional)  space  FP  of  all 
free  positions  of  fl,  and  is  therefore  a  problem  in  "computational  topology".  It  is  shown  [SS2] 
that  if  FP  is  a  semi-algebraic  set  (as  will  be  the  case  in  most  applications)  then  the  motion 

planning  problem  can  be  solved  in  time  polynomial  in  the  number  of  geometric  constraints 

,,-••    1-;  ;'lv    >.tic.~  -.:'.  ■ 
defining  FP  and  in  their  maximal  degree,  but  doubly  exponential  in  *  (this  result  has  been 

recently   improved  in   [KY]  who  have  given  an  algorithm   requiring  only  an  exponential 

:  ui  ;-Bq  'j~c  ns  :•■  '='  no  l  - 
space).  The  general  procedure  given  by  this  result,  which  is  based  on  a  decomposition 

..    c.  ';0  f>.c'-;o>.-     ,.  .i- jo'iiiii-.; 
technique  due  to  Collins  [Co]  and  originally  applied  to  Tarski's  theory  of  real  closed  fields,  is 

of  course  hopelessly  inefficient.  Moreover,  various  theoretical  analyses,  for  which  sec  [Re], 

[HJW2],  [HSS],  [JP],  indicate  that  the  general  motion-planning  problem  will  necessarily  be 

intractable  (i.e.  PSPACE-hard)  if  the  number  k  of  degrees  of  freedom  of  the  moving  system 

is  not  restricted. 

In  spite  of  these  negative  worst-case  results,  numerous  efficient  algorithms  for  planning 
the  motions  of  various  simple  robot  systems  have  been  found  (these  generally  apply  to  cases 
in  which  k  is  fixed;  with  this  restriction,  the  problem  is  known  to  be  polynomial  in  all  cases 
[SS2],  but  the  polynomial  exponents  involved  may  be  impractically  large).  Among  the 
relatively  efficient  algorithms  which  have  been  found  are: 

(i)  0(rt  log  n)  or  0(n  log^n)  algorithms  for  planning  purely  translational  planar  motions  for  a 
convex  body  moving  amidst  polygonal  obstacles  having  a  total  of  n  corners  ([OY],  [KS], 
[LS2]). 

(ii)  O(n^log  n)  algorithms  for  planning  a  planar  motion  of  a  rod  moving  in  a  similar  space 
([LSI],  [SiS];  cf.  also  [OSY2]). 
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(iii)  Various  other  results  on  3-D  motions  of  a  rod  amidst  polyhedral  obstacles,  coordinated 
planar  motions  of  several  circles,  etc.;  see  [SS3],  [SA],  [884],  [Ya]. 

Not  all  these  techniques  suffice  for  the  requirements  of  practice.  Their  complexity  tends 
to  increase  rapidly  when  the  number  of  degrees  of  freedom  of  B  increases.  For  systems 
having  six  degrees  of  freedom  (typical  for  many  commercial  robot  systems)  no  adequately 

C. 

efficient  motion  planning  algorithms  have  been  developed  as  yet.  Moreover,  most  known 
algorithms  (with  the  exception  of  [SiS],  see  below)  are  oriented  toward  the  worst  case 
behavior  of  the  problem  they  handle,  and  generally  fail  to  exploit  favorable  aspects  of  the 

obstacle   layout,   which  can  be  expected  to  be  very  helpful  in  most  practical  cases.   For 

.■moaxioq  ^mil  ni  '.    •    ■ 

example,  it  should  be  easier  to  plan  a  motion  in  cases  where  the  obstacles  are  well  separated 
than  in  cases  where  there  are  clusters  of  many  obstacles  forming  tight  passages  having  locally 
complex  geometries.  The  presence  of  an  obstacle  in  one  part  of  a  large  workspace  should  not 
greatly  complicate  the  planning  of  a  collision-free  motion  of  B  around  obstacles  in  a  second 
comer  of  the  workspace. 

This  note  will  suggest  a  simple  general  technique  for  obtaining  efficient  motion  planning 
algorithms  even  for  robot  systems  with  multiple  degrees  of  freedom,  at  least  common 
favorable  cases. 

Dennition  1:  A  set  of  obstacles  is  said  to  have  bounded  local  complexity  if  the  obstacles  are 
defined  by  a  collection  of  algebraic  equalities  and  inequalities  of  fixed  degree,  and  if  for 
some  (hence  all)  positive  radii  p,  the  number  of  obstacle  surfaces  intersecting  any  ball  S^  of 
radius  p  is  bounded  by  some  constant  M^.  Here  the  term  obstacle  surface  means  a  connected 
portion  of  the  boundary  of  an  obstacle.  (In  the  problems  considered,  each  of  these  obstacle 
surfaces  can  be  specified  by  any  collection  of  algebraic  equalities  and  inequalities.) 

Definition  2:  A  robot  system  B  with  internal  degrees  of  freedom  is  said  to  be  of  bounded  size 
if  the  maximum  diameter  of  B,  in  any  of  its  positions,  is  bounded  above  by  some  fixed 
constant  D  >  0  (which  will  be  called  the  size  of  the  system  B). 
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Theorem;  The  motion  of  a  robot  system  B  of  bounded.. si2e  moving  in  an  environment  of 
boimded  local  complexity  can  be  planned  in  time  linear  in  the  volume  of  the  environment. 
More  precisely,  if  the  environment  can  be  covered  by  K  open  cubes  in  such  a  way  that  each 
position  of  B  fits  into  one  of  these  cubes,  and  if  each  of  these  cubes  intersect  only  a  fixed 
number  of  other  cubes  and  only  L  obstacle  surfaces,  then  motion  of  B  can  be  planned  in  time 
0{K<^g(L)),  where  ^gim)  is  the  time  required  to  plan  the  motion  of  B  in  an  environment 
with  m  geometric  constraints,  each  spedfred  by  aii- algebraic  equality  or  inequality  of  fixed 
maximal  degree.  ''  ^^  i^''^'":-'-^^ 

Proof:  Let  C,,  .  .  .  ,Cjf  be  JC  open  cubes  covering  the  environment  such  that  every  position  of 
B  lies  within  one  of  these  cubes.  Then  by "ftie"Heihe-Borel  Theorem  there  exists  an  obstacle- 
avoiding  path  IT  connecting  two  positions  P  and  Q  of  fi  if  and  only  if  there  exists  a  sequence 
of  free  positions  P  =  Pi,P2>  •  >i-iP\i'^^  g^'sudinthat  for  each  /,  F,  and  i»,+  j  both  lie  within 
the  same  cube  Cj  and  the  portion  of  tiie'^btiondf  B  fti>m  /»,  to  P;+j  is  entirely  within  Cj 
(cover  the  path  ir  by  the  given  cubes  and  use  compactness  of  -n-). 

Let  FP(-  denote  the  space  cf  all  free  positions  of  B  within  any  one  of  the  cubes  C.  The 
bounded  Ioc£il  complexity  of  the  environment  implies  that  FP^  bas  only  a  constant  number  of 
connected  components  (the  constant  depending  on  the  local  complexity  of  the  environment), 
and  furthermore  all  these  components  can  be  computed  in  constant  time. 

We  can  thus  define  a  connectivity  graph  CG  whose  nodes  are  all  the  connected 
components  of  any  of  the  subspaces  FP(^,  and  in  which  an  edge  connects  a  connected 
component  -y,  of  FP^.    to  another  component  -yj  of  FP(-    (where  C,  ?t  Cj)  iff  C,  overlaps  Cj 

and  7|  n  72  ^  0  ('°  other  words,  iff  there  exists  a  position  of  B  wholly  contained  in 
C]  n  Cj  which  belongs  to  both  components  -y,,  72). 

Finally,  suppose  that  the  covering  of  the  environment  by  the  cubes  C  is  locally  finite, 
i.e.  at  most  some  fixed  M  cubes  can  overlap  any  cube  C  (see  below  for  one  possible  simple 
such  cover).    Then  the  nodes  of  CG  are  of  bounded  degree,  and  hence  CG  is  linear  in  the 


number  of  cubes  needed  to  cover  the  eovtronment,  as  asserted.   Q.E.D. 

To  obtain  a  locally  finite  cover  of  the  environment  by  cubes,  one  can  simply  take  all 
cubes  of  side  4D  whose  centers  lie  at  all  points  of  an  orthogonal  lattice  with  spacing  D,  where 
D  is  the  size  of  the  system  B.  Then  for  each  sphere  S  of  diameter  D  there  exists  a  cube  C 

whose  center  lies  within  distance  — —D  <  D  from  the  center  of  5,  so  that  S  is  contained  in 

2  ,    C.  1': 

C.  This  implies  that  each  position  of  B  is  fully  contaiaed  within  at  least  one  of  these  cubes. 
Moreover,  it  is  clear  that  each  of  these  cubes  intersects  at  most  some  fixed  number  of  other 
cubes,  so  that  this  cover  is  indeed  locally  finite,  as  required. 

The  following  additional  points  shau?d  b,e  noted„in  regard  to  implementation  of  the 
above  technique.  .     ,  (j  fart  'K  icoaiior'  < 

(i)  When  partitioning  the  environrurnt-intoicuhfeaaflirl  fir^p^have  to  identify  the  obstacle 
surfaces  intersecting  each  cube.  This  can  be  idone  ;effici|5ntiy  .using  known  orthogonal  range 
query  techniques  (for  a  description  of  which  see  e.gs  [Me])sr,    -  ■tr. 

(ii)  When  solving  the  motion  planning  problem  within  each  cube  separately,  one  should 
consider  the  boundary  of  the  cube  as  an  additional  obstacle,  so  as  to  ensure  that  only 
positions  of  B  within  that  cube  are  analyzed. 

(iii)  In  constructing  the  edges  of  the  connectivity  graph  CG  one  must  first  calculate  for  each 
cube  the  list  of  cubes  overlapping  it,  which  is  entirely  straightforward  due  to  the  regular 
arrangement  of  the  cubes.  Next,  for  each  pair  of  overlapping  cubes  C,,  Cj,  one  needs  to  find 
all  pairs  of  connected  components  -/,  of  FP^  and  72  °^  ^^c  which  have  nonempty 
intersection.  The  most  straightforward  way  of  doing  this  is  to  form  the  intersection  C  of  C, 
and  C2  and  to  apply  a  local  motion  planning  algorithm  to  find  all  the  connected  components  7 
of  FP(~.  Then  for  each  such  7  one  can  choose  a  position  Z  €  7,  find  the  components  7,  of 
FP(.  and  72  of  FP(-  which  contain  Z,  and  introduce  an  edge  of  CG  connecting  these  two 
nodes  of  CG .  It  is  easy  to  see  that  this  process  builds  all  the  edges  of  CG . 


(iv)  The  calculations  performed  in  (iii)  also  makes  it  possible  to  construct  a  path  between  two 
given  positions  of  B  when  such  a  path  existsi.  For  this  one  needs  to  transform  a 
(combinatorial)  path  it  in  CG  into  a  continuous  motion  of  B.  To  do  this,  first  note  that  the 
calculations  performed  in  (iii)  can  associate  a  position  Z  in  7,  n  -/j  ^i^^  ^^^h  edge  it  1,1 2)  °^ 
CG.  Then,  given  any  position  Zj  6  7j  and  another ^wsition  Z2  €  72>  ^^  *=*°  f'"*  Pl^°  *  P*^ 
within  "Y,  from  Zj  to  Z  and  combine  it  with  a  path  within  ^j  from  Z  to  Zj.  Required  motions 
are  then  easily  generated  by  iterating  this  elementary  step. 

(iv)  One  of  the  drawbacks  of  the  naive  technique  presented  above  is  that  it  does  not  exploit 
the  existence  of  obstacle  layouts 'wkflfli^  dS  nB^'change  across  large  distances.  For  example,  if 
the  environment  contains  a  long  narrow  corridor,  the  above  technique  will  cover  that  corridor 
by  many  cubes,  within  each  of  w^icfa  tttwilkruseimuch  the  same  local  motion  planning 
repeatedly.  It  may  even  cover  large  empty  portions  of  the  environment  by  cubes,  even 
though  within  each  of  which  motion,  planning,. is  trivial.  To  optimize  this  approach  one  wants 
to  detect  such  situations,  and  then  in  such  cases  to  use  larger  cubes  or  other  shapes  which  still 
intersect  only  a  small  number  of  obstacles.  Something  of  such  an  improved  approach  is 
implicit  in  the  recent  work  of  Sifroni  and  Sharir  mentioned  below. 

Remarks:  (1)  The  notion  of  bounded  local  complexity  as  presented  in  this  paper  is 
appropriate  to  situations  in  which  the  maximum  size  of  B  is  small  compared  to  the  size  of  its 
working  environment.  This  will  be  the  case  e.g.  if  B  is  a  mobile  robot  moving  along  a  factory 
floor.  However  there  are  certain  circumstances  in  which  our  definition  of  bounded  local 
complexity  requires  modifications.  For  example,  if  B  is  an  anthropomorphic  manipulator 
arm,  the  maximum  size  of  B  will  be  roughly  the  size  of  its  entire  workspace,  so  that  any  cube 
containing  some  positions  of  B  may  intersect  almost  all  obstacles  present  in  fl's  workspace. 
Wc  can  get  around  this  problem  by  redefining  bounded  local  complexity  in  this  case  to  mean 
e.g.  that  every  vertical  wedge  containing  the  first  vertical  link  of  the  arm  meets  only  a  fixed 
number  of  obstacles.  In  this  case  we  will  cover  the  workspace  of  B  by  such  wedges  rather 
than  by  cubes,   and  then  a  modified  version  of  the  general  technique  sketched  above  will 
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condnue  to  apply.  it  - 

(2)  As  noted  above,  the  concept  of  boimded  I iv^  complexity  is  implicit  in  a  recent  paper  by 

Sifroni  and  Sharir  [SiS]  on  motion  planning  foi'  a  rod  moving  in  2-space,  which  presents  an 

algorithm  that  runs  in  time  0(K  log  n)  where  n  is  the  total  number  of  obstacle  corners  and 

where  K  is  the  number  of  pairs  of  obstacle  corners  or  walls  lying  within  distance  d  from  each 

other,  d  being  the  length  of  the  rod.  j; 

■;  ■  »  lid 
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